clear all;
close all;
clc;

figureid = 1;

% sin / cos testing
N = 500;
angle = linspace(pi/-2, pi/2, N);

real_sin = sin(angle);
real_cos = cos(angle);
calc_sin = zeros(1, N);
calc_cos = zeros(1, N);
angle_res = zeros(1,N);

round_angle = int8(round(angle .* (2 .^ 6)));
for i = 1 : N
	data = [0x27, 0, round_angle(i)];
	data = ucordic8(data, 1, 1);
	calc_cos(i) = double(data(1)) ./ (2 .^ 6);
	calc_sin(i) = double(data(2)) ./ (2 .^ 6);
	angle_res(i) = double(data(3))./ (2 .^ 6);
endfor

figure(figureid);figureid = figureid + 1;
subplot(2,1,1);
plot(angle, real_sin, angle, calc_sin);
grid on; axis([-1.75 1.75 -1.5 1.5]);
title('Real Sine vs. Calculated Sine');
xlabel('theta(rad)');ylabel('value');legend('real', 'calculated');

subplot(2,1,2);
plot(angle, real_cos, angle, calc_cos);
grid on; axis([-1.75 1.75 -0.15 1.2]);
title('Real Cosine vs. Calculated Cosine');
xlabel('theta(rad)');ylabel('value');legend('real', 'calculated');
print -color -deps sin_cos_data.eps

err_sin = real_sin - calc_sin;
err_cos = real_cos - calc_cos;
figure(figureid);figureid = figureid + 1;
subplot(2,1,1);
plot(angle, err_sin, angle, err_cos);
grid on; axis([-1.75 1.75 -0.075 0.075]);
title('Error between real and calculated sine/cosine value');
xlabel('theta(rad)');ylabel('Error');legend('sin(theta)', 'cos(theta)');

subplot(2,1,2);
scatter(angle, angle_res);
grid on; axis([-1.75 1.75 -0.02 0.005]);
title('Residual of variable z');
xlabel('theta(rad)');ylabel('Residual');legend('z');
print -color -deps sin_cos_err.eps

% Multiply testing
N = 100;

Mz1 = linspace(-1, -0.5, N);
Mz2 = linspace(0.5, 1, N);
Mx = linspace(-1.5, 1.5, N);
real_prod1 = zeros(N, N);
real_prod2 = zeros(N, N);
round_Mz1 = int8(round(Mz1 .* (2 .^ 6)));
round_Mz2 = int8(round(Mz2 .* (2 .^ 6)));
round_Mx = int8(round(Mx .* (2 .^ 6)));
calc_prod1 = zeros(N, N);
calc_prod2 = zeros(N, N);
res_Mz1 = zeros(N, N);
res_Mz2 = zeros(N, N);

for j = 1 : N
	for i = 1 : N
		data1 = [round_Mx(i), 0x00, round_Mz1(j)];
		data2 = [round_Mx(i), 0x00, round_Mz2(j)];
		data1 = ucordic8(data1, 0, 1);
		data2 = ucordic8(data2, 0, 1);
		real_prod1(i, j) = Mx(i) .* Mz1(j);
		real_prod2(i, j) = Mx(i) .* Mz2(j);
		calc_prod1(i, j) = double(data1(2)) ./ (2 .^ 6);
		calc_prod2(i, j) = double(data2(2)) ./ (2 .^ 6);
		res_Mz1(i, j) = double(data1(3)) ./ (2 .^ 6);
		res_Mz2(i, j) = double(data2(3)) ./ (2 .^ 6);
	endfor
endfor

figure(figureid);figureid = figureid + 1;
subplot(2,1,1);
err_prod1 = abs(real_prod1 - calc_prod1);
surf(Mx, Mz1, err_prod1');
shading('interp'); view(2);
colorbar('EastOutside');
title('Error between real and calculated products: -1 < Mz < -0.5');
xlabel('Mx'); ylabel('-1 < Mz < -0.5');

subplot(2,1,2);
err_prod2 = abs(real_prod2 - calc_prod2);
surf(Mx, Mz2, err_prod2');
shading('interp'); view(2);
colorbar('EastOutside');
title('Error between real and calculated products: 0.5 < Mz < 1');
xlabel('Mx'); ylabel('0.5 < Mz < 1');
print -color -deps mul_err.eps

figure(figureid);figureid = figureid + 1;
subplot(2,2,1);
surf(Mx, Mz1, calc_prod1');
shading('interp'); view(2);
colorbar('EastOutside');
title('Calculated products: -1 < Mz < -0.5');
xlabel('Mx'); ylabel('-1 < Mz < -0.5');

subplot(2,2,2);
surf(Mx, Mz2, calc_prod2');
shading('interp'); view(2);
colorbar('EastOutside');
title('Calculated products: 0.5 < Mz < 1');
xlabel('Mx'); ylabel('0.5 < Mz < 1');

subplot(2,2,3);
surf(Mx, Mz1, real_prod1');
shading('interp'); view(2);
colorbar('EastOutside');
title('Real products: -1 < Mz < -0.5');
xlabel('Mx'); ylabel('-1 < Mz < -0.5');

subplot(2,2,4);
surf(Mx, Mz2, real_prod2');
shading('interp'); view(2);
colorbar('EastOutside');
title('Real products: 0.5 < Mz < 1');
xlabel('Mx'); ylabel('0.5 < Mz < 1');
print -color -deps mul_data.eps

figure(figureid);figureid = figureid + 1;
subplot(2,1,1);
surf(Mx, Mz1, res_Mz1');
shading('interp'); view(2);
colorbar('EastOutside');
colormap('gray');
title('Residual of Mz: -1 < Mz < -0.5');
ylabel('-1 < Mz < -0.5');xlabel('Mx');

subplot(2,1,2);
surf(Mx, Mz2, res_Mz2');
shading('interp'); view(2);
colorbar('EastOutside');
colormap('gray');
title('Residual of Mz: 0.5 < Mz < 1');
ylabel('0.5 < Mz < 1'); xlabel('Mx');
print -color -deps mul_residual.eps

% SQRT testing

N = 400;

val = linspace(0.25, 1, N);
val_p = val + 0.25;
val_n = val - 0.25;
round_val_p = int8(round(val_p .* (2 .^ 6)));
round_val_n = int8(round(val_n .* (2 .^ 6)));
real_sqrt = sqrt(val_p .^ 2 - val_n .^ 2);
calc_sqrt = zeros(1, N);
residual_y = zeros(1, N);

for i = 1 : N
	data = [round_val_p(i) round_val_n(i) 0];
	data = ucordic8(data, -1, 0);
	calc_sqrt(i) = double(data(1)) ./ (2 .^ 6);
	residual_y(i) = double(data(2)) ./ (2 .^ 6);
endfor

K = prod(sqrt(1 - (2.^ [-2 -4 -6 -8 -8 -10 -12 -14])));

norm_calc_sqrt = calc_sqrt ./ K;

figure(figureid);figureid = figureid + 1;
subplot(3, 1, 1);
plot(val, real_sqrt, val, norm_calc_sqrt);
title('SQRT Real vs. Calculated');
xlabel('Input');
ylabel('Value');
grid on;legend('Real', 'Calculated');
subplot(3, 1, 2);
plot(val, real_sqrt - norm_calc_sqrt);
title('SQRT Error');
xlabel('Input');
ylabel('Error');
grid on;
subplot(3, 1, 3);
plot(val, residual_y);
grid on;
title('Residual of Y');
xlabel('Input'); ylabel('Residual');
print -color -deps sqrt_graph.eps

% Devide testing

N = 400;

num1 = 0.25;
den1 = linspace(0.25, 1, N);
real_res1 = num1 ./ den1;
num2 = 0.0625;
den2 = linspace(0.0625, 0.25, N);
real_res2 = num2 ./ den2;
num3 = 0.015625;
den3 = linspace(0.015625, 0.0625, N);
real_res3 = num3 ./ den3;

round_num1 = int8(round(num1 .* (2 .^ 6)));
round_den1 = int8(round(den1 .* (2 .^ 6)));
round_num2 = int8(round(num2 .* (2 .^ 6)));
round_den2 = int8(round(den2 .* (2 .^ 6)));
round_num3 = int8(round(num3 .* (2 .^ 6)));
round_den3 = int8(round(den3 .* (2 .^ 6)));

calc_res1 = zeros(1, N);
calc_res2 = zeros(1, N);
calc_res3 = zeros(1, N);

residual_y1 = zeros(1, N);
residual_y2 = zeros(1, N);
residual_y3 = zeros(1, N);

for i = 1 : N
	data1 = [round_den1(i), round_num1, 0];
	data2 = [round_den2(i), round_num2, 0];
	data3 = [round_den3(i), round_num3, 0];
	data1 = ucordic8(data1, 0, 0);
	data2 = ucordic8(data2, 0, 0);
	data3 = ucordic8(data3, 0, 0);
	calc_res1(i) = double(data1(3)) ./ (2 .^ 6);
	calc_res2(i) = double(data2(3)) ./ (2 .^ 6);
	calc_res3(i) = double(data3(3)) ./ (2 .^ 6);
	residual_y1(i) = double(data1(2)) ./ (2 .^ 6);
	residual_y2(i) = double(data2(2)) ./ (2 .^ 6);
	residual_y3(i) = double(data3(2)) ./ (2 .^ 6);
endfor

figure(figureid);figureid = figureid + 1;
subplot(3,1,1);
plot(den1, real_res1, den1, calc_res1);
xlabel('Denominator from 0.25 to 1');
ylabel('Value');
title('Numerator is 0.25');
legend('Real', 'Calculated');
grid on;
subplot(3,1,2);
plot(den2, real_res2, den2, calc_res2);
xlabel('Denominator from 0.0625 to 0.25');
ylabel('Value');
title('Numerator is 0.0625');
legend('Real', 'Calculated');
grid on;
subplot(3,1,3);
plot(den3, real_res3, den3, calc_res3);
xlabel('Denominator from 0.015625 to 0.0625');
ylabel('Value');
title('Numerator is 0.0625');
legend('Real', 'Calculated');
grid on;
print -color -deps devide_value.eps

figure(figureid);figureid = figureid + 1;
subplot(3,1,1);
plot(den1, real_res1 - calc_res1);
xlabel('Denominator from 0.25 to 1');
ylabel('Error');
title('Numerator is 0.25');
grid on;
subplot(3,1,2);
plot(den2, real_res2 - calc_res2);
xlabel('Denominator from 0.0625 to 0.25');
ylabel('Error');
title('Numerator is 0.0625');
grid on;
subplot(3,1,3);
plot(den3, real_res3 - calc_res3);
xlabel('Denominator from 0.015625 to 0.0625');
ylabel('Error');
title('Numerator is 0.015625');
grid on;
print -color -deps devide_error.eps

figure(figureid);figureid = figureid + 1;
subplot(3,1,1);
plot(den1, residual_y1);
xlabel('Denominator from 0.25 to 1');
ylabel('Residual');
title('Numerator is 0.25');
grid on;
subplot(3,1,2);
plot(den2, residual_y2);
xlabel('Denominator from 0.0625 to 0.25');
ylabel('Residual');
title('Numerator is 0.0625');
grid on;
subplot(3,1,3);
plot(den3, residual_y3);
xlabel('Denominator from 0.015625 to 0.0625');
ylabel('Residual');
title('Numerator is 0.015625');
grid on;
print -color -deps devide_residual.eps

% sinh / cosh testing
N = 500;
D = linspace(-1 .* log(2), log(2), N);

real_cosh = cosh(D);
real_sinh = sinh(D);
calc_cosh = zeros(1, N);
calc_sinh = zeros(1, N);
res_D = zeros(1,N);

round_D = int8(round(D .* (2 .^  6)));

for i = 1 : N
	data = [0x4d, 0x00, round_D(i)];
	data = ucordic8(data, -1, 1);
	calc_cosh(i) = double(data(1)) ./ (2 .^  6);
	calc_sinh(i) = double(data(2)) ./ (2 .^  6);
	res_D(i) = double(data(3)) ./ (2 .^ 6);
endfor
figure(figureid);figureid = figureid + 1;
subplot(2,1,1);
plot(D, real_cosh, D, calc_cosh);
title('Real cosh vs. Calculated cosh');
xlabel('D');ylabel('value');
legend('Real', 'Calculated');
grid on; axis([-0.75 0.75 0.9 1.3]);

subplot(2,1,2);
plot(D, real_sinh, D, calc_sinh);
title('Real sinh vs. Calculated sinh');
xlabel('D');ylabel('value');
legend('Real', 'Calculated');
grid on; axis([-0.75 0.75 -0.85 0.85]);
print -color -deps sinh_cosh_data.eps

err_cosh = real_cosh - calc_cosh;
err_sinh = real_sinh - calc_sinh;
figure(figureid);figureid = figureid + 1;
subplot(2,1,1);
plot(D, err_cosh, D, err_sinh);
title('Error between real and calculated cosh/sinh');
xlabel('D');ylabel('Error');
legend('cosh', 'sinh');
grid on; axis([-0.75 0.75 -0.065 0.06]);

subplot(2,1,2);
scatter(D, res_D);
title('Residual of variable z');
xlabel('D');ylabel('Residual');
grid on; axis([-0.75 0.75 -0.02 0.005]);
print -color -deps sinh_cosh_err.eps

% atan testing
N = 500;

y = linspace(-1, 1, N);
round_y = int8(round(y .* (2 .^ 6)));

real_atan = atan(y);
calc_atan = zeros(1, N);
res_y = zeros(1, N);

for i = 1 : N
	data = [0x40, round_y(i), 0x00];
	data = ucordic8(data, 1, 0);
	calc_atan(i) = double(data(3)) ./ (2 .^ 6);
	res_y(i) = double(data(2)) ./ (2 .^ 6);
endfor

figure(figureid);figureid = figureid + 1;
plot(y, real_atan, y, calc_atan);
title('Real arctan vs. Calculated arctan');
xlabel('y'); ylabel('theta(rad)');
grid on; legend('Real', 'Calculated');
axis([-1.1 1.1 -1 1]);
print -color -deps atan_data.eps

err_atan = real_atan - calc_atan;
figure(figureid);figureid = figureid + 1;
subplot(2,1,1);
plot(y, err_atan); grid on;
title('Error between real and calculated arctan value');
xlabel('y');ylabel('Error(rad)');
axis([-1.1 1.1 -0.06 0.06]);

subplot(2,1,2);
scatter(y, res_y); grid on;
title('Residual of variable y');
xlabel('y'); ylabel('Residual');
grid on; axis([-1.1 1.1 -0.1 0.1]);
print -color -deps atan_err.eps

% atanh testing
N = 500;

y = linspace(0.17, 0.75, N);
round_y = int8(round(y .* (2 .^ 6)));

real_atanh = atanh(y);
calc_atanh = zeros(1, N);
res_y = zeros(1, N);

for i = 1 : N
	data = [0x40, round_y(i), 0x00];
	data = ucordic8(data, -1, 0);
	calc_atanh(i) = double(data(3)) ./ (2 .^ 6);
	res_y(i) = double(data(2)) ./ (2 .^ 6);
endfor

figure(figureid);figureid = figureid + 1;
plot(y, real_atanh, y, calc_atanh);
title('Real arctanh vs. Calculated arctanh');
xlabel('y'); ylabel('theta(rad)');
grid on; legend('Real', 'Calculated');
axis([0.1 0.8 0.15 1]);
print -color -deps atanh_data.eps

err_atanh = real_atanh - calc_atanh;
figure(figureid);figureid = figureid + 1;
subplot(2,1,1);
plot(y, err_atanh); grid on;
title('Error between real and calculated arctanh value');
xlabel('y');ylabel('Error(rad)');
axis([0.1 0.8 -0.1 0.1]);

subplot(2,1,2);
scatter(y, res_y); grid on;
title('Residual of variable y');
xlabel('y'); ylabel('Residual');
grid on; axis([0.1 0.8 -0.04 0.03]);
print -color -deps atanh_err.eps
